﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace robotics_ex_1_3
{
    class Room
    {
        private Polygon _polygon;

        public Room(Polygon polygon)
        {
            _polygon = polygon;
        }

        public List<Point> calcFreeSpace(Polygon robot)
        {
            List<Line> freeSpaceLines = new List<Line>();

            foreach (Line currentEdge in _polygon.Edges)
            {
                Line innerLine = getInnerLine(currentEdge, robot);

                Console.WriteLine(string.Format("The inner line of {0} is {1}\n", currentEdge, innerLine));

                freeSpaceLines.Add(innerLine);
            }

            return NewPolygonFinder.extract_polygon(freeSpaceLines);
        }

        private Line getInnerLine(Line edge, Polygon robot)
        {
            Point meetingPoint = robot.getFirstMeetingPoint(edge);

            Point vector = getTranslationVector(edge, meetingPoint, robot);
            Console.WriteLine(string.Format("The meeting point of {0} is {1}", edge, meetingPoint));

            return new Line(new Point(edge.StartPoint.X + vector.X, edge.StartPoint.Y + vector.Y),
                            new Point(edge.EndPoint.X + vector.X, edge.EndPoint.Y + vector.Y));
        }

        private Point getTranslationVector(Line edge, Point meetingPoint, Polygon robot)
        {
            if (edge.IsHorizontal)
            {
                return new Point(0, robot.RefPoint.Y - meetingPoint.Y);
            }

            if (edge.IsVertical)
            {
                return new Point(robot.RefPoint.X - meetingPoint.X, 0);
            }

            double high = meetingPoint.Y;

            double deltaX = high / edge.OrigTan;

            //Ax+By+c = 0
            double A = edge.OrigTan;
            double B = -1;
            double C = -A * (meetingPoint.X - deltaX);

            //Distance from line to point - |A*x1 + B * y1 + C| / sqrt(A^2 + B^2)
            double distance = Math.Abs(A * robot.RefPoint.X + B * robot.RefPoint.Y + C) / Math.Sqrt(A * A + B * B);

            //calculating the movement in every axes
            double angle = 90 - Math.Atan(edge.OrigTan);

            double xMovement = distance * Math.Sin(angle);
            double yMovement = distance * Math.Cos(angle);

            return new Point(xMovement, yMovement);
        }

    }
}
